import sensor, image, time
from pyb import Pin
import struct
from pyb import UART
import json
from pyb import LED
# You may need to tweak the above settings for tracking green things...
# Select an area in the Framebuffer to copy the color settings.

sensor.reset() # Initialize the camera sensor.
sensor.set_pixformat(sensor.RGB565) # use RGB565.
sensor.set_framesize(sensor.QQVGA) # use QQVGA for speed.
sensor.skip_frames(10) # Let new settings take affect.
sensor.set_auto_whitebal(False) # turn this off.
sensor.set_windowing((10,0,126,120))
clock = time.clock() # Tracks FPS.

uart =UART(3, 115200)  #串口3，波特率115200
uart.init(115200, bits=8, parity=None, stop=1)  #8位数据位，无校验位，1位停止

# For color tracking to work really well you should ideally be in a very, very,
# very, controlled enviroment where the lighting is constant...
red_threshold   = (19, 46, 34, 67, 19, 52)
blue_threshold   = (24, 54, -17, 16, -46, -25)
green_threshold   = (32, 56, -47, -14, 5, 45)
size_threshold = 2000
LED(1).on()
LED(2).off()
LED(3).off()
def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob[2]*blob[3] > max_size:
            max_blob=blob
            max_size = blob[2]*blob[3]
    return max_blob

def send_data_packet(x, y, h, m):
    temp = struct.pack("<biiiib",                #格式为俩个字符俩个整型
                   0xA5,
                   int(x), # up sample by 4    #数据1
                   int(y), # up sample by 4    #数据2
                   int(h), # up sample by 4    #数据2
                   int(m), # up sample by 4    #数据2
                   0x5A)
    uart.write(temp)                           #串口发送


while(True):
    clock.tick() # Track elapsed milliseconds between snapshots().
    img = sensor.snapshot() # Take a picture and return the image.

    blobs = img.find_blobs([red_threshold ,blue_threshold, green_threshold], merge=False)

    if blobs:
        max_blob = find_max(blobs)
        y_error = max_blob[5]-img.width()/2
        x_error = max_blob[6]-img.height()/2
        h_error = max_blob[2]*max_blob[3]
        if h_error>20 and h_error<3000:
            img.draw_rectangle(max_blob[0:4]) # rect
            img.draw_cross(max_blob[5], max_blob[6]) # cx, cy

            print("x error: ", x_error)
            print("y error: ", y_error)
            print("h error: ", h_error)
            print("color ", max_blob.code())
            send_data_packet(x_error, y_error, h_error ,max_blob.code())

            LED(1).off()
            LED(2).toggle()
            LED(3).off()



